import requests
import os
import argparse
import json
import cams
import time
import cv2
import numpy as np
import svar

osdk=svar.load('svar_osdk')

env=osdk.LinuxSetup(True)
vehicle = env.getVehicle()
rtmv=None
height=0
taskid=None

def callback_buf(buf):
  global rtmv
  global_pos=vehicle.broadcast.getGlobalPosition()
  height=global_pos["height"]
  if height < 50:
    return
  if rtmv is None:
    rtmv = open('/data/Dataset/rtmv/'+str(time.time())+".rtmv",'wb',buffering=0)
  mem=memoryview(buf)
  rtmv.write(mem.tobytes())

# start rtmv publish
options={}
options["camera_position"]=osdk.OSDK_CAMERA_POSITION_NO_1
options["playload_index"]=osdk.PAYLOAD_INDEX_0
options["h264_source"]=osdk.OSDK_CAMERA_SOURCE_H20T_WIDE
#options["h264_source"]=osdk.OSDK_CAMERA_SOURCE_H20T_ZOOM
options["enable_rtmv"]=True
node=osdk.VehicleNode(vehicle,options)
sub_rtmv=osdk.messenger.subscribe("rtmv",0,callback_buf)

gimbal_control = osdk.GimbalControl(vehicle)
flight_control = osdk.FlightControl(vehicle)

def stitch_process():
  # prepare task
  parameters={}
  parameters["map2dfusion.auto_zoom"]=2
  parameters["pointsfusion.use_terrainfusion"]=True
  parameters["terrainfusion.auto_zoom"]=0
  parameters["terrainfusion.max_zoom"]=30
  parameters["pointsfusion.clone_frame"]=0
  parameters["pointsfusion.dense_method"]=2
  parameters["pointsfusion.fulldp"]=0
  
  cameras = cams.cameras
  server='http://192.168.1.191:1024'
  task={"task":"online_points","parameters":parameters,"dataset":{"type":"images_stream","cameras":cameras,"images":[]}}
  headers = {'Content-Type': 'application/json;charset=UTF-8'}
  r = requests.request("post",server+"/task",json=task,headers=headers)
  print("task created with response",r.text)
  taskid=r.json().get('taskid')
  
  def post_image(taskid,imgfile):
    taskjson=json.dumps({"image":imgfile})
    jsonfile = os.path.splitext(os.path.split(imgfile)[1])[0]+'.json'
    files={
        "f.json" : (jsonfile,taskjson.encode()),"f.jpg" : open(imgfile, "rb")
       }
    r = requests.post(server+"/task/"+ taskid+"/frame", None, files=files)
    print(r.text)
  
  # start images fetching
  downloaded={}
  savefolder='.'
  is_first=False
  
  while(False):
    vehicle.cameraManager.setModeSync(osdk.PAYLOAD_INDEX_0,osdk.MEDIA_DOWNLOAD,2)
    vehicle.cameraManager.obtainDownloadRightSync(osdk.PAYLOAD_INDEX_0,True,2)
  
    file_list=vehicle.cameraManager.reqFileList(osdk.PAYLOAD_INDEX_0)
  
    media=file_list.media
    print("there are ",len(media),"files")
  
    for m in media:
      if m.fileName.find('WIDE') < 0:
        continue
      if m.fileName.find('198000000') > 0:
        print("abort",m)
        continue
      file_path=os.path.join(savefolder,m.fileName)
  
      if downloaded.get(m.fileIndex) != None:
        print("abort",m.fileName)
        continue
  
      if os.path.exists(file_path):
        continue
      if is_first:
        downloaded[m.fileIndex]=m
        continue
      print("reqesting file",file_path,"id",m.fileIndex)
      ret=vehicle.cameraManager.reqFileData(osdk.PAYLOAD_INDEX_0,m.fileIndex,file_path)
      print("reqest finished with code:",ret)
      if ret !=0:
        osdk.ErrorCode.printErrorCodeMsg(ret)
        break
      if taskid:
        post_image(taskid,file_path)
    
    if len(media) > 0:
      is_first=False
  
    time.sleep(3)
  
  if taskid:
    r = requests.request("post",server+"/task/"+ taskid+"/frame",json={},headers=headers)
    print(r.text)


#gimbalPYR = vehicle.broadcast.getGimbal()
#print("gimbalPYR: ", gimbalPYR)
#vehicle.gimbalManager.rotateSync(osdk.PAYLOAD_INDEX_0,{"pitch":0,"yaw":gimbalPYR["yaw"],"roll":gimbalPYR["roll"],"time":0.1},1)

from flask import Flask
from flask import request, jsonify, redirect, url_for, send_file, send_from_directory, render_template
app = Flask(__name__)

@app.route("/getGlobalPosition")
def getGlobalPosition():
    return jsonify(vehicle.broadcast.getGlobalPosition())

@app.route("/setBroadcastFreqDefaults")
def setBroadcastFreqDefaults():
    return jsonify(vehicle.broadcast.setBroadcastFreqDefaults())

@app.route("/getTimeStamp")
def getTimeStamp():
    return jsonify(vehicle.broadcast.getTimeStamp())

@app.route("/getQuaternion")
def getQuaternion():
    return jsonify(vehicle.broadcast.getQuaternion())

@app.route("/getVelocity")
def getVelocity():
    return jsonify(vehicle.broadcast.getVelocity())

@app.route("/getAngularRate")
def getAngularRate():
    return jsonify(vehicle.broadcast.getAngularRate())

@app.route("/getRelativePosition")
def getRelativePosition():
    return jsonify(vehicle.broadcast.getRelativePosition())

@app.route("/getGimbal")
def getGimbal():
    return jsonify(vehicle.broadcast.getGimbal())

@app.route("/getBatteryInfo")
def getBatteryInfo():
    return jsonify(vehicle.broadcast.getBatteryInfo())

@app.route("/rotateSync", methods=['POST', 'GET'])
def rotateSync():
    if request.method=='POST':
        pitch=float(request.form["pitch"])
        yaw=float(request.form["yaw"])
        roll=float(request.form["roll"])
        rtime=float(request.form["time"])
    else:
        pitch=float(request.args.get("pitch"))
        yaw=float(request.args.get("yaw"))
        roll=float(request.args.get("roll"))
        rtime=float(request.args.get("time"))
    vehicle.gimbalManager.rotateSync(osdk.PAYLOAD_INDEX_0,{"pitch":pitch,"yaw":yaw,"roll":roll,"time":rtime},1)
    return jsonify(vehicle.broadcast.getGimbal())

@app.route("/GimbalsetSpeed", methods=['POST'])
def GimbalsetSpeed():
    if request.method=='POST':
        pitch=int(request.form["pitch"])
        yaw=int(request.form["yaw"])
        roll=int(request.form["roll"])
        
        gimbal_control.set_gimbal_position(pitch, yaw, roll)
    return jsonify({"setstatus":"gimbal set completed"})


@app.route("/FlightControl", methods=['POST'])
def FlightControl():
    if request.method=='POST':
        xOffsetDesired=float(request.form["xOffsetDesired"])
        yOffsetDesired=float(request.form["yOffsetDesired"])
        zOffsetDesired=float(request.form["zOffsetDesired"])
 
        yawDesired=float(request.form["yawDesired"])
        posThresholdInM=float(request.form["posThresholdInM"])
        yawThresholdInDeg=float(request.form["yawThresholdInDeg"])

        flight_control.set_flight_position( xOffsetDesired, yOffsetDesired, zOffsetDesired,
                                            yawDesired, posThresholdInM, yawThresholdInDeg)

    return jsonify({"setstatus":"flight control completed"})


@app.route("/FlightControl_speed", methods=['POST'])
def FlightControl_speed():
    if request.method=='POST':
        offsetDesired_x=float(request.form["offsetDesired_x"])
        offsetDesired_y=float(request.form["offsetDesired_y"])
        offsetDesired_z=float(request.form["offsetDesired_z"])
 
        yawRate=float(request.form["yawRate"])
        timeMs=int(request.form["timeMs"])

        flight_control.set_flight_speed( offsetDesired_x, offsetDesired_y, offsetDesired_z,
                                         yawRate, timeMs)

    return jsonify({"setstatus":"flight control speed completed"})



@app.route("/getPYR")
def getPYR():
    quaternion = vehicle.broadcast.getQuaternion()
    print("quaternion: ", quaternion)
    print("type ", type(quaternion))
    print("type ", type(quaternion[0]))
    eular_angle = osdk.SO3(quaternion[0],quaternion[1],quaternion[2],quaternion[3])
    print("pitch: ", eular_angle.getPitch())
    print("yaw: ", eular_angle.getYaw())
    print("roll: ", eular_angle.getRoll())
        
    return jsonify({"pitch":str(eular_angle.getPitch()), "yaw":str(eular_angle.getYaw()), "roll":str(eular_angle.getRoll())})
    #return jsonify([eular_angle.getPitch(), eular_angle.getYaw(), eular_angle.getRoll()])


@app.route("/start_stitch")
def start_stitch():
  print(request.args)
  method=request.args.get("method")
  # prepare task
  parameters={}
  parameters["map2dfusion.auto_zoom"]=0
  parameters["pointsfusion.use_terrainfusion"]=True
  parameters["terrainfusion.auto_zoom"]=0
  parameters["terrainfusion.max_zoom"]=30
  parameters["pointsfusion.clone_frame"]=0
  parameters["pointsfusion.dense_method"]=2
  parameters["pointsfusion.fulldp"]=0
  
  cameras = cams.cameras
  server='http://192.168.1.232:1024'
  task={"task":method,"parameters":parameters,"dataset":{"type":"images_stream","cameras":cameras,"images":[]}}
  headers = {'Content-Type': 'application/json;charset=UTF-8'}
  r = requests.request("post","http://192.168.1.232:1024/task",json=task,headers=headers)
  print("task created with response",r.text)
  global taskid
  taskid=r.json().get('taskid')
  return jsonify(r.json())

@app.route("/stop_stitch")
def stop_stitch():
  
  global taskid
  if taskid != None:
    r = requests.request("post","http://192.168.1.232:1024/task/"+ taskid+"/frame",json={})
    print(r.text)
    return jsonify(r.json())
  return jsonify({"code":"UnkownTask"})

if __name__ == "__main__":
    app.run(host="0.0.0.0", port=1234)
